#pragma once
#include <ros/ros.h>
#include <rviz/panel.h>
#include <memory>
#include <thread>
#include <libssh/libssh.h>
#include <sensor_msgs/PointField.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>
#include <mutex>

namespace Ui
{
class NaviTeleop;
}

namespace rviz_plugins
{

	class TeleopPanel : public QWidget
	{
		Q_OBJECT
	public:
		TeleopPanel(QWidget* Parent = nullptr);
		~TeleopPanel() override;
		void startOrStopNav(int type);
		void startOrStopMapping(int type);
		void startOrStopLoc(int type);
		void startOrStopRobot(int type);
		void startOrStopKeyboard(int type); 
		void startOrStopRecord(int type);
		void tof_callback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg_in);
		void rdf_callback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg_in);
		void stereo_callback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg_in);
		void odom_callback(const nav_msgs::Odometry &odom_msg);
		void pointsSwitch(int type);
		void resetPoints();

		void keyPressEvent(QKeyEvent* Event) override;
		void keyReleaseEvent(QKeyEvent* Event) override;
		bool eventFilter(QObject *obj, QEvent *event) override;

		Ui::NaviTeleop* ui_{ nullptr };
		QTimer* timer_toggle_{ nullptr };
		std::unique_ptr<ros::NodeHandle> node_handle_{ nullptr };
		std::unique_ptr<ros::Publisher> start_loc_pub_{ nullptr };
		std::unique_ptr<ros::Publisher> twist_pub_{ nullptr };
		std::unique_ptr<ros::Publisher> recond3d_pub_{ nullptr };
		float linear_{ 0.0 };
		float angular_{ 0.0 };
		ssh_session session;
		int line=0;// 0:nothing, 1:front, -1:behind
		int angle=0;//0:nothing, 1:left, -1:right
		std::thread th_publish_, th_publish_2;
		bool twist_switch=false;
		ros::Subscriber tof_sub, rdf_sub, stereo_sub, odom_sub;
		int pt_type=0;//0:tof, 1:rdf, 2:stereo
		std::vector<uint8_t> tof_vecs, rdf_vecs, stereo_vecs;
		std::mutex tof_mut, rdf_mut, stereo_mut;
		bool sync_time;
	};
} // end namespace rviz_plugins
